from typing import Dict, Any, Optional
import logging
import time
import json
from dataclasses import dataclass

# 配置日志
logging.basicConfig(level=logging.INFO)
logger = logging.getLogger("drone_control")

class DroneCommandError(Exception):
    """无人机命令执行错误"""
    pass

@dataclass
class DroneState:
    """无人机状态数据类"""
    drone_id: int
    connected: bool = False
    flight_status: str = "idle"  # idle, connected, flying, landing, error
    battery_level: int = 100
    latitude: Optional[float] = None
    longitude: Optional[float] = None
    altitude: Optional[float] = None
    speed: float = 5.0
    heading: float = 0.0
    last_command_time: Optional[float] = None

class DroneController:
    """无人机控制器"""
    
    def __init__(self):
        # 存储无人机状态
        self.drones: Dict[int, DroneState] = {}
        # 命令映射
        self.command_handlers = {
            "takeoff": self._handle_takeoff,
            "land": self._handle_land,
            "hover": self._handle_hover,
            "move": self._handle_move,
            "rotate": self._handle_rotate,
            "stop": self._handle_stop,
            "set_speed": self._handle_set_speed,
            "camera_control": self._handle_camera_control
        }
    
    def connect(self, drone_id: int, initial_battery: int = 100) -> DroneState:
        """
        连接无人机
        
        Args:
            drone_id: 无人机ID
            initial_battery: 初始电量
            
        Returns:
            无人机状态
        """
        if drone_id in self.drones:
            if self.drones[drone_id].connected:
                raise DroneCommandError(f"无人机 {drone_id} 已经连接")
            # 更新连接状态
            self.drones[drone_id].connected = True
            self.drones[drone_id].flight_status = "connected"
        else:
            # 创建新的无人机状态
            self.drones[drone_id] = DroneState(
                drone_id=drone_id,
                connected=True,
                flight_status="connected",
                battery_level=initial_battery
            )
        
        logger.info(f"无人机 {drone_id} 连接成功")
        return self.drones[drone_id]
    
    def disconnect(self, drone_id: int) -> bool:
        """
        断开无人机连接
        
        Args:
            drone_id: 无人机ID
            
        Returns:
            是否成功断开
        """
        if drone_id not in self.drones or not self.drones[drone_id].connected:
            raise DroneCommandError(f"无人机 {drone_id} 未连接")
        
        self.drones[drone_id].connected = False
        self.drones[drone_id].flight_status = "idle"
        logger.info(f"无人机 {drone_id} 断开连接")
        return True
    
    def get_status(self, drone_id: int) -> DroneState:
        """
        获取无人机状态
        
        Args:
            drone_id: 无人机ID
            
        Returns:
            无人机状态
        """
        if drone_id not in self.drones:
            raise DroneCommandError(f"无人机 {drone_id} 不存在")
        
        # 模拟电量消耗
        self._simulate_battery_drain(drone_id)
        
        return self.drones[drone_id]
    
    def execute_command(self, drone_id: int, command: str, params: Dict[str, Any]) -> Dict[str, Any]:
        """
        执行无人机命令
        
        Args:
            drone_id: 无人机ID
            command: 命令类型
            params: 命令参数
            
        Returns:
            执行结果
        """
        if drone_id not in self.drones:
            raise DroneCommandError(f"无人机 {drone_id} 不存在")
        
        drone = self.drones[drone_id]
        
        if not drone.connected:
            raise DroneCommandError(f"无人机 {drone_id} 未连接")
        
        # 检查电池电量
        if drone.battery_level < 15 and command != "land":
            raise DroneCommandError(f"无人机电量过低 ({drone.battery_level}%)，只能执行降落命令")
        
        # 检查命令是否支持
        if command not in self.command_handlers:
            raise DroneCommandError(f"未知命令: {command}")
        
        # 执行命令
        try:
            result = self.command_handlers[command](drone, params)
            drone.last_command_time = time.time()
            return {
                "success": True,
                "command": command,
                "message": result["message"],
                "parameters": params
            }
        except Exception as e:
            logger.error(f"执行命令 {command} 失败: {str(e)}")
            return {
                "success": False,
                "command": command,
                "message": str(e),
                "parameters": params
            }
    
    def _handle_takeoff(self, drone: DroneState, params: Dict[str, Any]) -> Dict[str, str]:
        """处理起飞命令"""
        if drone.flight_status != "connected" and drone.flight_status != "idle":
            raise DroneCommandError(f"当前状态无法起飞: {drone.flight_status}")
        
        # 设置默认高度
        height = params.get("height", 10)
        drone.flight_status = "flying"
        drone.altitude = height
        
        logger.info(f"无人机 {drone.drone_id} 起飞，目标高度: {height}m")
        return {"message": f"起飞命令已发送，目标高度: {height}m"}
    
    def _handle_land(self, drone: DroneState, params: Dict[str, Any]) -> Dict[str, str]:
        """处理降落命令"""
        if drone.flight_status != "flying":
            raise DroneCommandError(f"当前状态无法降落: {drone.flight_status}")
        
        drone.flight_status = "landing"
        drone.altitude = 0
        
        logger.info(f"无人机 {drone.drone_id} 开始降落")
        return {"message": "降落命令已发送"}
    
    def _handle_hover(self, drone: DroneState, params: Dict[str, Any]) -> Dict[str, str]:
        """处理悬停命令"""
        action = params.get("action")
        
        if action == "return_to_home":
            drone.flight_status = "flying"
            logger.info(f"无人机 {drone.drone_id} 开始返航并悬停")
            return {"message": "返航悬停命令已发送"}
        else:
            if drone.flight_status != "flying":
                raise DroneCommandError(f"当前状态无法悬停: {drone.flight_status}")
            logger.info(f"无人机 {drone.drone_id} 悬停")
            return {"message": "悬停命令已发送"}
    
    def _handle_move(self, drone: DroneState, params: Dict[str, Any]) -> Dict[str, str]:
        """处理移动命令"""
        if drone.flight_status != "flying":
            raise DroneCommandError(f"当前状态无法移动: {drone.flight_status}")
        
        direction = params.get("direction")
        if not direction:
            raise DroneCommandError("移动命令缺少方向参数")
        
        # 验证方向
        valid_directions = ["forward", "backward", "left", "right", "up", "down"]
        if direction not in valid_directions:
            raise DroneCommandError(f"无效的方向: {direction}，有效方向: {', '.join(valid_directions)}")
        
        # 计算移动距离
        distance = params.get("distance", 5.0)
        
        # 更新位置（模拟）
        logger.info(f"无人机 {drone.drone_id} 向 {direction} 移动 {distance}m")
        
        # 这里应该有实际的位置更新逻辑
        if direction == "up" and drone.altitude is not None:
            drone.altitude += distance
        elif direction == "down" and drone.altitude is not None:
            drone.altitude = max(0, drone.altitude - distance)
        
        return {"message": f"移动命令已发送，方向: {direction}，距离: {distance}m"}
    
    def _handle_rotate(self, drone: DroneState, params: Dict[str, Any]) -> Dict[str, str]:
        """处理旋转命令"""
        if drone.flight_status != "flying":
            raise DroneCommandError(f"当前状态无法旋转: {drone.flight_status}")
        
        angle = params.get("angle", 0)
        drone.heading = (drone.heading + angle) % 360
        
        logger.info(f"无人机 {drone.drone_id} 旋转 {angle} 度，当前朝向: {drone.heading} 度")
        return {"message": f"旋转命令已发送，角度: {angle} 度"}
    
    def _handle_stop(self, drone: DroneState, params: Dict[str, Any]) -> Dict[str, str]:
        """处理停止命令"""
        drone.flight_status = "idle"
        
        logger.info(f"无人机 {drone.drone_id} 停止所有动作")
        return {"message": "停止命令已发送"}
    
    def _handle_set_speed(self, drone: DroneState, params: Dict[str, Any]) -> Dict[str, str]:
        """处理设置速度命令"""
        speed = params.get("speed")
        if speed is None:
            raise DroneCommandError("设置速度命令缺少速度参数")
        
        # 验证速度范围
        if not (0 < speed <= 40):
            raise DroneCommandError("速度必须在 0-40 m/s 之间")
        
        drone.speed = speed
        logger.info(f"无人机 {drone.drone_id} 设置速度为 {speed} m/s")
        return {"message": f"速度设置命令已发送，速度: {speed} m/s"}
    
    def _handle_camera_control(self, drone: DroneState, params: Dict[str, Any]) -> Dict[str, str]:
        """处理相机控制命令"""
        action = params.get("action", "snap")
        
        logger.info(f"无人机 {drone.drone_id} 相机控制: {action}")
        
        if action == "snap":
            return {"message": "拍照命令已发送"}
        elif action == "record_start":
            return {"message": "开始录像命令已发送"}
        elif action == "record_stop":
            return {"message": "停止录像命令已发送"}
        else:
            return {"message": f"相机控制命令已发送，动作: {action}"}
    
    def _simulate_battery_drain(self, drone_id: int):
        """模拟电池消耗"""
        drone = self.drones[drone_id]
        if not drone.connected:
            return
        
        current_time = time.time()
        if drone.last_command_time:
            # 根据状态消耗电量
            if drone.flight_status == "flying":
                # 飞行状态耗电更快
                drain_rate = 0.1  # 每秒消耗0.1%
            else:
                # 其他状态耗电较慢
                drain_rate = 0.01  # 每秒消耗0.01%
            
            time_diff = current_time - drone.last_command_time
            battery_drain = time_diff * drain_rate
            drone.battery_level = max(0, drone.battery_level - battery_drain)
        
        drone.last_command_time = current_time

# 创建全局控制器实例
drone_controller = DroneController()